/*
 * @Author: LVGRAPE
 * @Date: 2023-05-18 11:19:07
 * @LastEditTime: 2023-09-14 15:47:37
 * @LastEditors: LVGRAPE
 * @Description: 
 * @FilePath: \ZINO_FC_V4\ZINO\filter\filter.c
 * 可以输入预定的版权声明、个性签名、空行等
 */
#include <rtthread.h>
#include "filter.h"
#include "maths.h"
#include "math.h"
#define M_PI_F (float)3.14159265


void KalmanFilterInit(Kalman_t *kfp)
{
    kfp->Last_P = 1;			
	kfp->Now_P = 0;		
	kfp->out = 0;			
	kfp->Kg = 0;		
	kfp->Q = 0;
	kfp->R = 0.1;
}
/**
 * @brief 
 * 
 * @param kfp 
 * @param q 过程噪声
 * @param r 测量噪声
 */
void KalmanFilterInitParam(Kalman_t *kfp, float q, float r)
{
    kfp->Last_P = 1;			
	kfp->Now_P = 0;		
	kfp->out = 0;			
	kfp->Kg = 0;		
	kfp->Q = q;
	kfp->R = r;
}

/**
 *卡尔曼滤波器
 *@param 	Kalman *kfp 卡尔曼结构体参数
 *   			float input 需要滤波的参数的测量值（即传感器的采集值）
 *@return 滤波后的参数（最优值）
 */
float KalmanFilter(Kalman_t *kfp,float input)
{
   //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
   kfp->Now_P = kfp->Last_P + kfp->Q;
   //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
   kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);
   //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
   kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值
   //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
   kfp->Last_P = (1-kfp->Kg) * kfp->Now_P;
   return kfp->out;
}


/**
 * 设置二阶低通滤波截至频率
 */
void lpf2pSetCutoffFreq(lpf2pData* lpfData, float sample_freq, float cutoff_freq)
{
	float fr = sample_freq/cutoff_freq;
	float ohm = tan_approx(M_PI_F/fr);
	float c = 1.0f+2.0f*cos_approx(M_PI_F/4.0f)*ohm+ohm*ohm;
	lpfData->b0 = ohm*ohm/c;
	lpfData->b1 = 2.0f*lpfData->b0;
	lpfData->b2 = lpfData->b0;
	lpfData->a1 = 2.0f*(ohm*ohm-1.0f)/c;
	lpfData->a2 = (1.0f-2.0f*cos_approx(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
	lpfData->delay_element_1 = 0.0f;
	lpfData->delay_element_2 = 0.0f;
}

/**
 * 二阶低通滤波
 */
void lpf2pInit(lpf2pData* lpfData, float sample_freq, float cutoff_freq)
{
	if (lpfData == NULL || cutoff_freq <= 0.0f) 
	{
		return;
	}

	lpf2pSetCutoffFreq(lpfData, sample_freq, cutoff_freq);
}



float lpf2pApply(lpf2pData* lpfData, float sample)
{
	float delay_element_0 = sample - lpfData->delay_element_1 * lpfData->a1 - lpfData->delay_element_2 * lpfData->a2;
	if (!isfinite(delay_element_0)) 
	{
		// don't allow bad values to propigate via the filter
		delay_element_0 = sample;
	}

	float output = delay_element_0 * lpfData->b0 + lpfData->delay_element_1 * lpfData->b1 + lpfData->delay_element_2 * lpfData->b2;

	lpfData->delay_element_2 = lpfData->delay_element_1;
	lpfData->delay_element_1 = delay_element_0;
	return output;
}

float lpf2pReset(lpf2pData* lpfData, float sample)
{
	float dval = sample / (lpfData->b0 + lpfData->b1 + lpfData->b2);
	lpfData->delay_element_1 = dval;
	lpfData->delay_element_2 = dval;
	return lpf2pApply(lpfData, sample);
}